#include <ros/ros.h>
#include <fstream>
#include "sensor_msgs/Imu.h"
#include "tf/transform_datatypes.h"
#include "geometry_msgs/PoseStamped.h"
#include <nav_msgs/Odometry.h>
#include <tf/transform_listener.h>
#include "sensor_msgs/LaserScan.h"

#define TURN_ANGLE 8.6 //degrees
const double thresh_ang = TURN_ANGLE * (3.1416/180.0);


std::ofstream myfile;
using namespace std;
nav_msgs::Odometry odom;
ros::Publisher pub;

double time_prev;
double time_last, xlast = 0, ylast = 0, yaw_last = 0,  mark=0,  delta_t, vel_linear, vel_angular, accel, curr_yaw;
double r,p=0,y, initial_yaw, dist_curr, start_turn_angle ,end_turn_angle, initial_x, initial_y; tf::Quaternion quat;
ros::Time start_time, end_time;
bool is_first = 1; bool is_first1 = 1, first_time_rec = 0, end_time_rec = 0;
int iter = 0; bool counting = 0;

bool datum_time_rec = 0;
double first_time_imu,time_last_imu, timestamp;

bool first_pose_rec=0, end_pose_rec=0;
geometry_msgs::Pose start_pose, end_pose;

double cma_lin_v=0,sig_lin_v=0,std_dev_lin_v,ox, oy, lx, ly, dx,dy,dt,lin_v, cma_yaw=0,sig_yaw=0,std_dev_yaw;
double dx1,dy1;
int lin_v_c=0, yaw_c=0; bool prev_state_save=1; geometry_msgs::PoseStamped prev_pose;
ros::Time now;
std::vector<double> yaws,vels;

sensor_msgs::LaserScan laser_scan;
float min_range;


double normalize(double in)
{
if(in < -M_PI) return in + 2*M_PI;
else if(in > M_PI) return in - 2*M_PI;
else return in;
}

void imuCallback(const sensor_msgs::ImuConstPtr& msg)
{
  tf::quaternionMsgToTF(msg->orientation, quat);
  tf::Matrix3x3(quat).getRPY(r,p,y); 
 
double angle_test=y;
//myfile << angle_test<< "\n"; 


  if(!datum_time_rec) { 
	first_time_imu = time_last_imu = ros::Time::now().toSec();
	datum_time_rec = 1;
  }

// Initial yaw record
  if(is_first)
  {
	initial_yaw = y; ROS_INFO("Datum yaw %f deg", y*57.3);
	is_first = 0;
  }
  
// Start turning 
if(fabs(y-initial_yaw) >= thresh_ang && !first_time_rec)
	{
		start_turn_angle = y;	
		start_time = ros::Time::now(); ROS_INFO("Start time recorded on angle %f",start_turn_angle*57.3);
		first_time_rec = 1;
	}
 
// End turning
if(y >= (start_turn_angle + (3.141/2.036)))    
	{
		end_turn_angle = y; 
		ROS_INFO("End time recorded on angle %f", end_turn_angle*57.3);
		end_time = ros::Time::now();
		end_time_rec = 1;
	}

//Time
timestamp = (ros::Time::now().toSec()-first_time_imu);  //ROS_INFO("AT TIME %f", timestamp);
//ROS_INFO("linear speed=  %f, anglular speed=  %f", vel_linear, vel_angular); 
myfile << timestamp << "	" << angle_test*57.3 <<  "\n";  
// End Time



//Standard deviation for the orientation
if(!is_first && !end_time_rec)
  {
	cma_yaw = (y + yaw_c*cma_yaw) / (double)(yaw_c+1); yaw_c++;
	yaws.push_back(y);
  }
}

void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg)
{

 if(first_time_rec && !first_pose_rec)
  {
	prev_pose.pose = start_pose = msg->pose;
	prev_state_save = 0;
	prev_pose.header.stamp = ros::Time::now();
	first_pose_rec = 1; ROS_INFO("Start pose recorded");
  }
  if(end_time_rec && !end_pose_rec)
  {
	end_pose = msg->pose;
	end_pose_rec = 1; ROS_INFO("End pose recorded");
  }

  if(!prev_state_save && first_pose_rec) prev_state_save = 1;
  if(prev_state_save && first_pose_rec && !end_time_rec)
  {
	now = ros::Time::now();
	dx = msg->pose.position.x-prev_pose.pose.position.x;  
	dy = msg->pose.position.y-prev_pose.pose.position.y;
	
	
	dt = (now-prev_pose.header.stamp).toSec();
	if(dt>0.0)
	{
		lin_v = sqrt(dx*dx+dy*dy)/dt;
		cma_lin_v = (lin_v + lin_v_c*cma_lin_v) / (double)(lin_v_c+1); lin_v_c++;
		vels.push_back(lin_v);
		prev_pose.header.stamp = now; prev_pose.pose = msg->pose;
	}
  }
}

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{

    std::vector<float> laser;
    laser = msg->ranges;

    int size_laser = laser.size();
    for (int i=0;i<size_laser;i++){
        if (laser[i] < 0.01){
            laser[i] = 99999;
        }
        if (laser[i] > 45){
            laser[i] = 99999;
        }
    }

    min_range = 2;
    int index_min;
    for (int i=0;i<size_laser;i++){
        if (laser[i] < min_range){
            min_range = laser[i];
            index_min = i;
	   //ROS_INFO("Minimum Range = %f", min_range);
        }
    }

    for (int j=0;j<size_laser;j++){
        if (laser[j] > min_range + 0.5){
            laser[j] = 0;
        }
    }

    laser_scan = *msg;
    laser_scan.ranges.clear();
    laser_scan.ranges = laser;
}



int main(int argc, char **argv)
{

  myfile.open ("/home/bojan/POW/Assessment/WST/90degturn.txt");
  myfile << "timestamp" << "	" << "Angle yaw" <<  "\n"; 


  ros::init(argc, argv, "imu_odom");
  ros::NodeHandle node;
  ros::NodeHandle nh;
  ros::Subscriber sub2 = nh.subscribe("slam_out_pose", 1, poseCallback);
  ros::Subscriber sub =  nh.subscribe("raw_imu_throttle", 1, imuCallback);
  ros::Subscriber sub3 =  nh.subscribe("scan_throttle", 1, scanCallback);
  ros::Publisher laser_pub = nh.advertise<sensor_msgs::LaserScan>("closest_points", 1);


while(!end_pose_rec && ros::ok()) ros::spinOnce();


		laser_pub.publish(laser_scan);
		dx = end_pose.position.x-start_pose.position.x;
  		dy = end_pose.position.y-start_pose.position.y;

 		double dist_tot = sqrt(dx*dx+dy*dy), time = (end_time-start_time).toSec(), avg_lin_v = dist_tot/time;

		

	
  		for(size_t i=0; i<yaws.size(); i++) sig_yaw += pow(yaws[i]-cma_yaw, 2);
  		std_dev_yaw = sqrt(sig_yaw/yaw_c)*57.3;
  		for(size_t i=0; i<vels.size(); i++) sig_lin_v += pow(vels[i]-cma_lin_v, 2);
 		 std_dev_lin_v = sqrt(sig_lin_v/lin_v_c);

 	 	ROS_INFO("Std dev --- Orientation: %f, Linear speed: %f", std_dev_yaw, std_dev_lin_v);
  		ROS_INFO("avarege linear velocity is %f, total dist %f", avg_lin_v, dist_tot);
  		ROS_INFO("Travelled %f seconds", time);
		//myfile << angle_test<< "\n"; 
		//ROS_INFO("Minimal distance to any obstale %f meters", min_range);
  


ros::spin();
 return 0;
}


















